#include "Steering.h"

// TODO: optimize this fucking mess
void Steering::Seek(NPCEntity* source, vector3df target)
{

	if (debug[Debug::SEEK_DISPLAY])
	{
		zone z;

		std::cout << "Entity Zone: ";
		if (World::getMap()->getZoneFromVector3df(source->getNode()->getPosition(), z))
			std::cout << z.getName();
		else
			std::cout << "N/A";
		std::cout << "\tEntity pos:" << source->getNode()->getPosition() << std::endl;
	}


	Map* m = World::getMap();
	std::list<segment_t> seg_lst = m->getSegmentsToPassThrough(source->getNode()->getPosition(), target );

	vector3df dest;

	if (seg_lst.begin() != seg_lst.end())
	{
		segment_t s = *seg_lst.begin();
		dest = s.center() - source->getNode()->getPosition();
/*
		// Check if node position is on the fucking segment
		irr::core::line3df seg_line(s.first, s.second);
		seg_line.start.Y = seg_line.end.Y = 0.0f;
		vector3df node_point = source->getNode()->getPosition();
		node_point.Y = 0.0f;
		if (seg_line.isPointBetweenStartAndEnd(node_point))
		{
			// L'entitee est pile poil sur le segment; peut poser des problemes de pathfinding, redirection au segment suivant
			std::list<segment_t>::iterator it = seg_lst.begin();
			++it;
			if (it != seg_lst.end())
				dest = it->center() - source->getNode()->getPosition();
			else
				dest = target - source->getNode()->getPosition();
		
		}
*/
	}
	else
		dest = target - source->getNode()->getPosition();


	Face(source, dest);
	dest.Y = 0;
	dest.normalize();
	dest *= source->getAttribute(std::string("speed")) * World::getWorld()->elapsedTime();

	source->setAnimation(EMAT_RUN);

	Face(source,  source->getNode()->getPosition() + dest); 

	source->getNode()->setPosition( source->getNode()->getPosition() + dest );

//	WallAvoidance(source);
    	collisionAvoidance(source);
	source->target_seek__ = target;
}

void Steering::Seek(NPCEntity* source, Entity* target)
{
	Face(source, target);
	Seek(source, target->getNode()->getPosition());
}

void Steering::Face(NPCEntity* source, Entity* target)
{
	Face(source, target->getNode()->getPosition());
}

void Steering::Face(NPCEntity* source, vector3df target)
{
	vector3df dest = target - source->getNode()->getPosition();
	source->getNode()->setRotation( vector3df(0.0f, 180.0f * atan2f(-dest.Z, dest.X) / M_PI, 0.0f) );
}

void Steering::Face(NPCEntity* source, float yaw)
{
	source->getNode()->setRotation( vector3df(0.0f, yaw, 0.0f) );
}

void Steering::Flee(NPCEntity* source, Entity* target)
{
	Steering::Flee(source, target->getNode()->getPosition());
}

void Steering::Flee(NPCEntity* source, vector3df target)
{
	vector3df seeking_target;
	vector3df dest;
	// Search a segment from which the distance is maximal from where the target is
	segment_t seg;

	//if (World::getMap()->getFarestSegment(target, seg) == false)
	//{
		vector3df seeking_direction = source->getNode()->getPosition() - target;
		seeking_direction.normalize();
		seeking_direction *= 100;
		Seek(source, source->getNode()->getPosition() + seeking_direction);
		//Face(source, source->getNode()->getPosition() + seeking_direction);
	/*}
	else
	{
		std::list<segment_t> seg_lst = World::getMap()->getSegmentsToPassThrough(source->getNode()->getPosition(), seeking_target);
		if (seg_lst.begin() != seg_lst.end())
		{
			segment_t s = *seg_lst.begin();
			dest = s.center() - source->getNode()->getPosition();
		}
		else
			dest = target - source->getNode()->getPosition();
		dest.Y = 0;
		dest.normalize();
		dest *= source->getAttribute(std::string("speed")) * World::getWorld()->elapsedTime();

		source->setAnimation(EMAT_RUN);

		Face(source, target);
		source->getNode()->setPosition( source->getNode()->getPosition() + dest );
	}*/
	WallAvoidance(source);
	collisionAvoidance(source);
}

void Steering::collisionAvoidance(NPCEntity* source)
{
     //avoid distance
	f64 radius= source->getMeshNode()->getBoundingBox().getExtent().getLength() -20 ;
	//f64 shortestTime=1000000.0;
	World* w=World::getWorld();
	Entity * firstTarget = 0;
	f64 firstMinSeparation=200;
	//f64 firstDistance;
	vector3df firstRelativePosition;
	vector3df relativePos;
	f64 distance=1000;

	ISceneNode* node=source->getNode();
	int lastID = node->getID();
	node->setID(5);
	std::vector<Entity*> entities;
	vector3df start = node->getPosition();
	vector3df end=vector3df(distance,0,0);
  //creates the 3 ray to detect a collision
	end.rotateXZBy(-node->getRotation().Y, vector3df(0, 0, 0));
	Entity* result1=World::getMap()->RayCastForEntity(node,start,end);

	end.rotateXZBy(-20,vector3df(0, 0, 0));
	Entity* result2=World::getMap()->RayCastForEntity(node,start,end);

	end.rotateXZBy(40,vector3df(0, 0, 0));
	Entity* result3=World::getMap()->RayCastForEntity(node,start,end);

 //if 2 rays detect a collsion with the same guy
	if(result1)
	{
		if(result2)
		{
			if(result1==result2)
			{
				entities.push_back(result1);
			}
		}
		if(result3)
		{
			if(result1==result3)
			{
				entities.push_back(result1);
			}
		}
	}
	node->setID(lastID);

	f64 move = source->getAttribute(std::string("speed")) * w->elapsedTime();
	std::vector<Entity*>::iterator it;
	//if we have detected 1 or several collisions
	if(entities.size()!=0)
	{
		for(it=entities.begin();it!=entities.end();it++)
		{
			Entity* pelo = *it ;
			if(pelo)
			{
                    //detects the distance between the source and the guy
				relativePos = pelo->getNode()->getPosition()-source->getNode()->getPosition();
				distance=relativePos.getLength();
				f64 minSeparation=distance-move;
				//if we are to close to a guy
				if(minSeparation <= radius)
				{     //detects the first collision                 
					if (minSeparation<firstMinSeparation)
					{
						firstTarget=pelo;
						firstMinSeparation=minSeparation;
						firstRelativePosition=relativePos;
					}
				}
			}
		}

		if((firstTarget))
		{
//			std::cout<<"firstTarget=="<<firstTarget->getType().c_str()<<std::endl;
          //avoid the collision with a flee
			vector3df direction = source->getNode()->getPosition() - firstTarget->getNode()->getPosition();
			direction.Y=0;
			direction.normalize();
            direction *= source->getAttribute(std::string("speed")) * World::getWorld()->elapsedTime();
            source->setAnimation(EMAT_RUN);

            //Face(source, direction);
	        source->getNode()->setPosition( source->getNode()->getPosition() + direction );
			
		}
	}
}

void Steering::WallAvoidance(NPCEntity* source)
{
     f64 avoidDistance= source->getMeshNode()->getBoundingBox().getExtent().getLength() +20 ;
     ITriangleSelector * selector=World::getMap()->getSelector();
     ISceneNode* node=source->getNode();
     
     line3d<f32> line;                  //main ray
     f64 distance=300;
     vector3df start = node->getPosition();
     line.start=start ;
     vector3df end=vector3df(distance,0,0);
     line.end=line.start+end;
     vector3df intersection;
	 triangle3df tri;
     vector3df normal;
     vector3df target;
     //checks if the main ray detects a collision
     if (World::getSceneManager()->getSceneCollisionManager()->getCollisionPoint(line, selector, intersection, tri))
		{
                  normal= tri.getNormal();
                  //creates a target to seek in order to avoid the wall
                  target=intersection+(normal*avoidDistance);                          
                  vector3df direction = target - source->getNode()->getPosition();
			      direction.Y=0;
			      direction.normalize();
                  direction *= source->getAttribute(std::string("speed")) * World::getWorld()->elapsedTime();
                  source->setAnimation(EMAT_RUN);

                  //Face(source, direction);
	              source->getNode()->setPosition( source->getNode()->getPosition() + direction );   
                  Face(source,  source->getNode()->getPosition() + direction );                                   
       
       }
       
       //creates 2 wiskers to check if there are undetected collisions
       end=vector3df(100,0,0);
       end.rotateXZBy(40,vector3df(0, 0, 0));
       line3d<f32> rightWisker(start,start+end);
       line3d<f32> leftWisker(start, start-end);
       vector3df intersection2;
	   triangle3df tri2;
	   //checks if the right wisker detects a collision
	   if(World::getSceneManager()->getSceneCollisionManager()->getCollisionPoint(rightWisker, selector, intersection2, tri2))
	   { 
                      //flee the collision point                                                                         
                  vector3df direction = source->getNode()->getPosition()-intersection2;
			      direction.Y=0;
			      direction.normalize();
                  direction *= source->getAttribute(std::string("speed")) * World::getWorld()->elapsedTime();
                  source->setAnimation(EMAT_RUN);

                  //Face(source, direction);
	              source->getNode()->setPosition( source->getNode()->getPosition() + direction );
                  Face(source,  source->getNode()->getPosition() + direction ); 
                          
       }       
       else
       //checks if the left wisker detects a collision
           if(World::getSceneManager()->getSceneCollisionManager()->getCollisionPoint(leftWisker, selector, intersection2, tri2))
           {
                          vector3df direction = source->getNode()->getPosition()-intersection2;
			      direction.Y=0;
			      direction.normalize();
                  direction *= source->getAttribute(std::string("speed")) * World::getWorld()->elapsedTime();
                  source->setAnimation(EMAT_RUN);

                  //Face(source, direction);
	              source->getNode()->setPosition( source->getNode()->getPosition() + direction );
	              Face(source,  source->getNode()->getPosition() + direction ); 
           }                                                                   
}

